#include "test.h"
#include "fdcan.h"
#include "motor_cybergear.h"
#include "plan.h"
#include "tim.h"
#include "user_math.h"

void setUp(void) {}

void tearDown(void) {}

float tic(void)
{
    HAL_GPIO_WritePin(TEST_IO_GPIO_Port, TEST_IO_Pin, GPIO_PIN_SET);
    return (float)__HAL_TIM_GET_COUNTER(&htim_test) * TIMER_COEF;
}

float toc(void)
{
    HAL_GPIO_WritePin(TEST_IO_GPIO_Port, TEST_IO_Pin, GPIO_PIN_RESET);
    return (float)__HAL_TIM_GET_COUNTER(&htim_test) * TIMER_COEF;
}

void testJointPlanner(void)
{
    JointPlanner_TypeDef *planner = joint_planner_new();
    int ret = joint_planner_init(0.0f, 1.0f, 0.0f, 0.0001f, planner);
    float t = 0.0f, p;
    float start_time, end_time;

    while (1)
    {
        start_time = tic();
        t += 0.1f;
        p = joint_plan_move(planner, t);
        end_time = toc();
        LOG_INFO("position = %f, cost %f ms", p, (end_time - start_time));
        TEST_ASSERT_EQUAL_FLOAT(0.0f, 0.0f);
        HAL_Delay(100);
    }
}

void test_mit_mode(void)
{
    uint8_t TxData[8] = {0x6c, 0x00, 0x90, 0x00, 0x08, 0x00, 0x05, 0x00};

    CAN_Send(0x01800001, TxData);
}

void test_enable_and_disable(void)
{
    for (unsigned int i = 0; i < 3; i++)
    {
        motor_enable(0x01);
        HAL_Delay(1000);
        motor_disable(0x01);
        HAL_Delay(1000);
    }
}

void test_mit_sin(void)
{
    MotorMIT_Typedef motor_mit;
    motor_mit.forward_torque = 0.0f;
    motor_mit.target_position = 0.0f;
    motor_mit.target_velocity = 0.0f;
    motor_mit.kp = 80.0f;
    motor_mit.ki = 0.2f;

    float freq = 1.0f, amplitude = 1.0f, t = 0.0f;
    float start_time = 0.0f, end_time = 0.0f;
    float last_target = 0.0f;
    motor_set_mode(0x01, MODE_MIT);
    HAL_Delay(100);

    while (1)
    {
        //        t += (end_time - start_time) * 0.001f;
        t += 0.002f;
        //        start_time = tic();
        motor_mit.target_position =
            -0.5f * amplitude * cosf(M_2PI_F * freq * t) + 0.5f * amplitude;
        motor_mit.target_velocity =
            0.5f * amplitude * M_2PI_F * freq * sinf(M_2PI_F * freq * t);
        //        motor_mit.target_velocity = (motor_mit.target_position - last_target) / t;
        motor_control_mit(0x01, &motor_mit);
        //        last_target = motor_mit.target_position;
        HAL_Delay(2);
        //        end_time = toc();
        //        LOG_DEBUG("cost %f ms", (end_time - start_time));
    }
}

// void test_pos_sin(void)
//{
//     float freq = 1.0f, amplitude = 1.0f, t = 0.0f;
//     float start_time = 0.0f, end_time = 0.0f;
//     float target_position = 0.0f;
//     motor_set_mode(0x01, MODE_POS);
//     HAL_Delay(100);
//
//     while (1)
//     {
//         //        t += (end_time - start_time) * 0.001f;
//         t += 0.002f;
//         start_time = tic();
//         motor_mit.target_position = -0.5f * amplitude * cosf(M_2PI_F * freq * t) + 0.5f * amplitude;
//         motor_set_position(0x01, motor_mit.target_position);
//         HAL_Delay(2);
//         end_time = toc();
//         //        LOG_DEBUG("cost %f ms", (end_time - start_time));
//     }
// }

// void test_vel_sin(void)
//{
//     float freq = 20.0f, amplitude = 1.0f, t = 0.0f;
//     float start_time = 0.0f, end_time = 0.0f;
//     //    float target_position = 0.0f;
//     motor_set_mode(0x01, MODE_VEL);
//     HAL_Delay(100);
//
//     while (1)
//     {
//         t += (end_time - start_time) * 0.001f;
//         //        t += 0.002f;
//         start_time = tic();
//         motor_mit.target_velocity = amplitude * sinf(M_2PI_F * freq * t);
//         motor_set_velocity(0x01, motor_mit.target_velocity);
//         HAL_Delay(2);
//         end_time = toc();
//         //        LOG_DEBUG("cost %f ms", (end_time - start_time));
//     }
// }

void run_test(void)
{
    //    test_mit_mode();
    //    test_enable_and_disable();
    test_mit_sin();
    //    test_pos_sin();
    //    test_vel_sin();
}
